#pragma once
#include <functional>
#include <map>
#include <memory>
#include <queue>
#include <stack>
#include <vector>

namespace Graph
{

namespace Algorithm
{

class GraphInterface
{
  public:
    GraphInterface( ) = default;
    ~GraphInterface( ) = default;
};

template < typename _Coordinate > class Search
{
  public:
    // typedef std::vector< _Coordinate > ( *neighbors_fun_handle )(
    //     const GraphInterface &graph, const _Coordinate &current );

    typedef std::function< std::vector< _Coordinate >(
        const GraphInterface &graph, const _Coordinate &current ) >
        neighbors_fun_handle;

    typedef std::function< double( const _Coordinate & ) > f_cost_handle;
    // typedef double ( *f_cost_handle )( const _Coordinate & );

  private:
    class Cost_Coordinate
    {
      public:
        double cost;
        _Coordinate coor;

        Cost_Coordinate( const double &cost, const _Coordinate &coor )
            : cost( cost ), coor( coor )
        {
        }
        bool operator<( const Cost_Coordinate &rhs ) const
        {
            return cost < rhs.cost;
        }

        bool operator>( const Cost_Coordinate &rhs ) const
        {
            return cost > rhs.cost;
        }
    };

  public:
    /**
     * @brief A*搜索算法。要求模板_Coordinate必须是可以比较大小的。该算法只是A*算法的一个模板，
     * 是否是A*算法依赖于估价函数的设计。估价函数f_cost=g_cost时，退化为bfs算法；f_cost=h_cost时，是为贪心算法；
     * f_cost=g_cost+h_csot且h_cost大于当前节点到目标节点的距离时退化为A算法；
     * f_cost=g_cost+h_cost时，且h_cost小于等于当前节点到目标节点的距离时，即是A*算法。
     * @return 一条可行路径。无法保证一定能找到一个路径，需自行检查返回值中是否有目标位置存在。只有bfs和A*算法才能
     * 保证找到一条最短路径。
     * @param graph [in] 图结构
     * @param start [in] 初始位置
     * @param goal [in] 目标位置
     * @param neighbors_fun [in] 提取当前节点下属节点的函数
     * @param f_cost_fun[in] 估价函数
     */
    static std::vector< _Coordinate > a_star_search( const GraphInterface &graph,
                                                const _Coordinate &start,
                                                const _Coordinate &goal,
                                                neighbors_fun_handle neighbors_fun,
                                                f_cost_handle f_cost_fun )
    {
        //TODO 这种记录模式要求_Coordinate是可比较大小的，待改进
        std::map< _Coordinate, _Coordinate > came_from;
        std::priority_queue< Cost_Coordinate, std::vector< Cost_Coordinate >,
                             std::greater< Cost_Coordinate > >
            frontier; //最小堆

        came_from[ start ] =
            start; //同时记录了某个节点是否被访问过且记录了访问路径

        frontier.push( Cost_Coordinate( 0, start ) );

        while ( !frontier.empty( ) )
        {
            auto current = frontier.top( );
            frontier.pop( );

            // 遍历当前节点的每一个邻居 bfs模型
            auto neighbors = neighbors_fun( graph, current.coor );
            for ( auto &&i : neighbors )
            {
                auto ret = came_from.find( i );
                if ( ret == came_from.end( ) ) //该节点没有被访问过
                {
                    double cost = f_cost_fun( current.coor ); //计算代价

                    came_from[ i ] = current.coor;

                    frontier.push( Cost_Coordinate( cost, i ) );

                    if ( i == goal )
                    {
                        goto finished;
                    }
                }
            }
        }

    finished:
        auto _goal = came_from.find( goal );
        if ( _goal == came_from.end( ) )
        {
            return std::vector< _Coordinate >( );
        }

        std::vector< _Coordinate > path;
        _Coordinate current = goal;
        while ( current != start )
        {
            path.push_back( current );
            current = came_from[ current ];
        }
        path.push_back( start );

        return path;
    }

    /**
     * @brief 深度优先搜索算法。不保证找到最短路径，但保证找到一个路径。
     * @param graph [in] 图结构
     * @param start [in] 初始位置
     * @param goal [in] 目标位置
     */
    // static Path dfs( const GraphInterface &graph, const _Coordinate &start,
    //                  const _Coordinate &goal )
    // {
    //     Path came_from;
    //     std::stack< _Coordinate > frontier;

    //     came_from[ start ] = start;
    //     frontier.push( start );
    //     while ( !frontier.empty( ) )
    //     {
    //         auto current = frontier.top( );
    //         frontier.pop( );

    //         // 遍历当前节点的每一个邻居 bfs模型
    //         auto neighbors = graph.neighbors( current );
    //         for ( auto &&i : neighbors )
    //         {
    //             auto ret = came_from.find( i );
    //             if ( ret == came_from.end( ) ) //该节点没有被访问过
    //             {
    //                 frontier.push( i );
    //                 came_from[ i ] = current;
    //                 if ( i == goal )
    //                 {
    //                     goto finished;
    //                 }
    //             }
    //         }
    //     }
    // finished:
    //     return came_from;
    // }
};

} // namespace Algorithm

} // namespace Graph
